import subprocess
import time
from .log_utils import setup_ros_logging

class ROSUtils:
    def __init__(self, log_dir, max_log_files=50):
        self.log_dir = log_dir
        self.max_log_files = max_log_files
        self.ros_processes = {}
    
    def call_rosservice(self, service_name, args=None):
        """调用ROS服务"""
        try:
            if args:
                cmd = ['rosservice', 'call', service_name, f"data: '{args}'"]
            else:
                cmd = ['rosservice', 'call', service_name]
            result = subprocess.run(cmd, capture_output=True, text=True, timeout=5*60)
            
            if result.returncode == 0:
                if 'success: True' in result.stdout or 'success: true' in result.stdout:
                    return {"code": 200, "message": "操作成功", "data": result.stdout}
                elif 'code: True' in result.stdout or 'code: true' in result.stdout:
                    return {"code": 200, "message": "操作成功", "data": result.stdout}
                else:
                    print(f"失败.{result.stdout}")
                    return {"code": 200, "message": "操作失败", "data": result.stdout}
            else:
                print(f"失败.{result.stderr}")
                return {"code": 500, "message": f"服务调用失败: {result.stderr}", "data": None}
                
        except subprocess.TimeoutExpired:
            print(f"服务调用超时")
            return {"code": 408, "message": "服务调用超时", "data": None}
        except Exception as e:
            print(f"服务调用异常: {str(e)}")
            return {"code": 500, "message": f"服务调用异常: {str(e)}", "data": None}

    def call_rostopic(self, topic_name, message_type, data):
        """发布ROS话题"""
        try:
            if message_type == 'UInt16'or message_type == 'Bool' or message_type == 'Int32':
                cmd = ['rostopic', 'pub', '-1', topic_name, f'std_msgs/{message_type}', str(data)]
            elif message_type == 'Status':
                cmd = ['rostopic', 'pub', '-1', topic_name, f'ss_common_msgs/{message_type}', str(data)]
            for tries in range(3):
                result = subprocess.run(cmd, capture_output=True, text=True, timeout=10)
                if result.returncode == 0:
                    return {"code": 200, "message": "消息发布成功", "data": None}
                time.sleep(0.5)
            return {"code": 500, "message": f"消息发布失败: {result.stderr}", "data": None}

        except Exception as e:
            return {"code": 500, "message": f"消息发布异常: {str(e)}", "data": None}
    
    def start_ros_nodes(self, launch_files):
        """启动ROS节点"""
        from .process_utils import kill_all_ros_processes
        
        # 先清理现有进程
        kill_all_ros_processes()
        time.sleep(2)
        
        success_count = 0
        failed_launches = []
        
        for launch_file in launch_files:
            try:
                # 设置日志重定向
                log_file, log_path = setup_ros_logging(launch_file, self.log_dir, self.max_log_files)
                
                # 启动ROS节点并重定向输出到日志文件
                process = subprocess.Popen(
                    ['roslaunch', 'ss_common_msgs', launch_file],
                    stdout=log_file,
                    stderr=log_file
                )
                
                self.ros_processes[launch_file] = {
                    'process': process,
                    'log_file': log_file,
                    'log_path': log_path
                }
                
                time.sleep(1)
                success_count += 1
                
            except Exception as e:
                failed_launches.append(f"{launch_file}: {str(e)}")
        
        # 等待进程启动
        time.sleep(5)
        return success_count, failed_launches
    
    def stop_ros_nodes(self):
        """停止所有ROS节点"""
        from .process_utils import kill_process_tree, kill_all_ros_processes
        
        # 关闭日志文件
        for launch_file, info in self.ros_processes.items():
            if 'log_file' in info and info['log_file']:
                try:
                    info['log_file'].close()
                except:
                    pass
        
        # 杀死进程
        for launch_file, info in self.ros_processes.items():
            if 'process' in info and info['process']:
                try:
                    kill_process_tree(info['process'].pid)
                except:
                    pass
        
        # 清理ROS核心进程
        kill_all_ros_processes()
        
        self.ros_processes.clear()
    
    def are_ros_processes_running(self):
        """检查ROS进程是否在运行"""
        if not self.ros_processes:
            return False
        
        for launch_file, info in self.ros_processes.items():
            if 'process' not in info or info['process'].poll() is not None:
                return False
        
        return True
    
    def get_ros_status(self):
        """获取ROS节点状态"""
        status_details = {}
        running_count = 0
        
        for launch_file, info in self.ros_processes.items():
            if 'process' in info and info['process'].poll() is None:
                status_details[launch_file] = "运行中"
                running_count += 1
                print(f"{launch_file} 运行中")
            else:
                status_details[launch_file] = "已停止"
                print(f"{launch_file} 已停止")
        
        return running_count, status_details